\section{Controller} \label{sec:sm}

\begin{center}
\begin{tikzpicture}[shorten >=1pt,node distance=5cm,on grid,auto] 
   \node[state,initial] (EN)   {Exit Nest}; 
   \node[state,thick] (EX) [below=of EN] {Explorer};
   \node[state,thick] (EC) [right=of EN] {Chain Following};
   \node[state,accepting,thick] (SR) [right=of EC] {Spot Reached};
   \node[state,accepting,thick] (CE) [below =of EC] {Chain End};
   \node[state,accepting,thick] (CM) [right=of CE] {Chain Member};
   \node[state,accepting,thick] (CJ) [below left=of CM] {Chain Junction};
    \path[->] 
    (EN) edge [bend right]  node [left,text width=2cm]  {Out of nest $\wedge \text{ } n_b = 0$} (EX)
         edge [bend left]  node [above,text width=2cm]  {$n_b >0$} (EC)
    (EX) edge [bend left]  node [above,rotate=45,text width=2cm]  {$n_b >0$} (EC)
         edge [bend right]  node [below,text width=2cm]  {$n_b = 0 \text{ } \wedge$ Out of Nest $\wedge \text{ } P_{etob}$} (CE)
          edge  node [above left,rotate=90]  {In Nest} (EN)
    (CE) edge node  {$n_b > 1$} (CM)
           edge [bend left] node [left,text width=1cm]  {$d_{cb} < 30[cm]$} (EC)
    (CM) edge [bend left] node [text width=2cm] {$n_b > 2$} (CJ)
    (EC) edge  node [above,text width=1.5cm]  {In Nest} (EN)
           %edge  node [above,rotate=45]  {$n_b > 1 \text{ } \wedge$ Out of Nest} (EX) 
           edge [bend left] node [right,text width=2cm] {$n_b == 1 \text{ } \wedge$ Out of Nest $\wedge \text{ } d_{cb} > d_c$} (CE)
           edge  node {On spot} (SR);
\end{tikzpicture}
\captionof{figure}{Implemented FSM for the \emph{s-bot} chain formation behavior}
\end{center}

The behavior of the robot in each state is detailed in \nameref{subsec:states}, reporting in each case the resulting virtual force $\mathbf{F_{res}}$ applied to the robot.

The nodes marked as final (i.e. those having the double border) corresponds to the states where the robot has stopped moving.

\paragraph{Parameters}
\begin{center}
\begin{tabular}{|c|c|c|}
\hline
\textbf{Parameter} & \textbf{Description} & \textbf{Value} \\ \hline
$v_{wheel}$ & Wheel velocity (straight movement) & $10 [\frac{cm}{s}]$ \\ \hline
$P_{etob}$ & Probability of starting a chain while being an explorer & 0.05 \\ \hline
$d_{chain}$ & Minimum distance among elements in the chain  & 130 [cm] \\ \hline
$\omega_{ds}$ & Angular rotation velocity of the distance sensor & $2\pi [\frac{rad}{s}]$ \\ \hline
$t_{ns}$ & Delay time on the decision to stop outside the nest & 100 [steps] \\ \hline
\end{tabular}
\captionof{table}{Controller parameters overview}
\label{tab:parameters}
\end{center}

\subsection{States} \label{subsec:states}
\paragraph{Exit nest} \label{par:exitnest}
\begin{equation}
  \mathbf{F_{res}} = \mathbf{F_{obs}} + \mathbf{F_{ds}} + \mathbf{F_{rab}}
\end{equation}

The exit nest state is the initial state of the robots, which is maintained as long as it remains in the nest.
The robots uses the distance scanner and proximity sensors to find their way out 
of the nest.

In addition to those forces, the robot is also attracted (by means of $\mathbf{F_{rab}}$) 
towards the closest robots in the RAB sensing range that are in the \emph{Explorer} 
or the \emph{Chain following} state.
This is done beacause such kind of robots are either directing or already 
outside of the nest, thus following them will guide a robot in the right 
direction.

If a robots senses any kind of beacon it changes its state to \emph{Chain following}.

Otherwise, it passes in \emph{Explorer} state.

\paragraph{Explorer}
\begin{equation}
  \mathbf{F_{res}} = \mathbf{F_{obs}} + \mathbf{F_{ds}} + \mathbf{F_{str}}
\end{equation}
In the \emph{Explorer} state, if no beacon is sensed, the robot moves outsides the nest without making any decision for $t_{ns}$ time steps, then deciding, at each simulation step, whether to stop or not.

The decision is stochastic and occurs with probability $P_{etob}$.

Otherwise, the robot goes straight ($\mathbf{F_{str}} = (1.0,0)$).

In any case, if the agent goes back to the nest, it enters the \emph{Exit Nest} state.

\paragraph{Chain following} \label{par:chainfollowing}
\begin{equation}
  \mathbf{F_{res}} = \mathbf{F_{obs}} + \mathbf{F_{ds}} + \mathbf{F_{str}}
\end{equation}
The \emph{Chain Following} state simply consists of a random walk of the robot 
in the environment.

The robots normally goes straight (guided by $\mathbf{F_{str}} = (1.0,0)$).

Its direction is then modified by the perceived obstacles both in a short range ($\mathbf{F_{obs}}$) and in a long range 
($\mathbf{F_{ds}}$).

As soon as the distance of a robot from the closest beacon is greater than the minimum chain distance ($d_{cb} > d_c$)
and exactly one beacon is sensed nearby ($n_b == 1$) (cfr. \nameref{par:rules} (2)) 
the robots stops and becomes a \emph{Chain End}.

If the robot loses contact with a chain beacon, it turns itself of 180 degrees. before 
restarting exploration.

In any case, if the agent goes back to the nest, it enters the \emph{Exit Nest} state.


\paragraph{Chain End}
\begin{equation}
  \mathbf{F_{res}} = (0,0)
\end{equation}
The \emph{Chain End} state is reached when a robot connects himself to the current end of the chain, becoming thus the new termination.

In this state, the robot stands still and broadcast information concerning its state, id and chain\_id to the neighboring robots.

If any beacon is sensed within 30 cm the robot starts moving again, in order to 
find a better position where to attach in the chain.

The transition to the \emph{Chain Member} state occurs anytime another agent connects to the current one.

\paragraph{Chain Member}
\begin{equation}
  \mathbf{F_{res}} = (0,0)
\end{equation}
A \emph{Chain Member} only acts as a beacon, broadcasting state information and acting as a relay for the back-propagation of the \emph{Target found} message.
A \emph{Chain Member} robot becomes a \emph{Chain Junction} if another robot decides to 
join the chain by attaching to it.

\paragraph{Chain Junction}
\begin{equation}
  \mathbf{F_{res}} = (0,0)
\end{equation}
The sole purpose of a \emph{Chain Junction} is to differentiate itself from the other beacons and stop the back-propagation of the \emph{Target found} message.

